{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "hairy-humidity",
   "metadata": {
    "id": "hairy-humidity"
   },
   "source": [
    "<center>\n",
    "<h4>CDS 110, Lecture 1</h4>\n",
    "<font color=blue><h1>Dynamics and Control of a Servomechanism System using Python-Control</h1></font>\n",
    "<h3>Richard M. Murray, Winter 2024</h3>\n",
    "</center>\n",
    "\n",
    "[Open in Google Colab](https://colab.research.google.com/drive/1GKRYwtbHWSWc21EIYYIZUnbJqUorhY8w)\n",
    "\n",
    "In this lecture we show how to model an input/output system and design a controller for the system (using eigenvalue placement).  This main intent of this lecture is to introduce the Python Control Systems Toolbox ([python-control](https://python-control.org)) and how it can be used to design a control system.\n",
    "\n",
    "We consider a class of control systems know as *servomechanisms*.  Servermechanisms are mechanical systems that use feedback to provide high precision control of position and velocity.  Some examples of servomechanisms are shown below:\n",
    "\n",
    "| | | |\n",
    "| -- | -- | -- |\n",
    "| Satellite Dish | Disk Drive | Robotics |\n",
    "| <img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/satellite-dish.png\" height=200 alt=\"Satellite Dish\"> | <img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/disk-drive.png\" height=200 alt=\"Disk Drive\"> | <img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/robotic-arm.png\" height=200 alt=\"Disk Drive\">\n",
    "| [YouTube video](https://www.youtube.com/watch?v=HSGfE_sC2hw) | [YouTube video](https://www.youtube.com/watch?v=oQh8KDea6SI) | [YouTube video](https://www.youtube.com/watch?v=hg3TIFIxWCo)\n",
    "| | |"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2c284896-bcff-4c06-b80d-d9d6fbc0690f",
   "metadata": {},
   "source": [
    "The python-control toolbox can be installed using `pip` over from conda-forge.  The code below will import the control toolbox either from your local installation or via pip.  We use the prefix `ct` to access control toolbox commands:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "invalid-carnival",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Import standard packages needed for this exercise\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "try:\n",
    "  import control as ct\n",
    "  print(\"python-control\", ct.__version__)\n",
    "except ImportError:\n",
    "  !pip install control\n",
    "  import control as ct"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "P7t3Nm4Tre2Z",
   "metadata": {
    "id": "P7t3Nm4Tre2Z"
   },
   "source": [
    "## System dynamics\n",
    "\n",
    "Consider a simple mechanism consisting of a spring loaded arm that is driven by a  motor, as shown below:\n",
    "\n",
    "<center><img src=\"https://www.cds.caltech.edu/~murray/courses/cds110/sp2024/servomech-diagram.png\" width=200 alt=\"servomech-diagram\"></center>\n",
    "\n",
    "The motor applies a torque that twists the arm against a linear spring and moves the end of the arm across a rotating platter. The input to the system is the motor torque $\\tau_\\text{m}$. The force exerted by the spring is a nonlinear function of the head position due to the way it is attached.\n",
    "\n",
    "The equations of motion for the system are given by\n",
    "\n",
    "$$\n",
    "J \\ddot \\theta = -b \\dot\\theta - k r\\sin\\theta + \\tau_\\text{m},\n",
    "$$\n",
    "\n",
    "which can be written in state space form as\n",
    "\n",
    "$$\n",
    "\\frac{d}{dt} \\begin{bmatrix} \\theta \\\\ \\theta \\end{bmatrix} =\n",
    "  \\begin{bmatrix} \\dot\\theta \\\\ -k r \\sin\\theta / J - b\\dot\\theta / J \\end{bmatrix}\n",
    "  + \\begin{bmatrix} 0 \\\\ 1/J \\end{bmatrix} \\tau_\\text{m}.\n",
    "$$\n",
    "\n",
    "The system parameters are given by\n",
    "\n",
    "$$\n",
    "k = 1,\\quad J = 100,\\quad b = 10,\n",
    "\\quad r = 1,\\quad l = 2,\\quad \\epsilon = 0.01.\n",
    "$$\n",
    "\n",
    "and we assume that time is measured in milliseconds (ms) and distance in centimeters (cm).  (The constants here are made up and don't necessarily reflect a real disk drive, though the units and time constants are motivated by computer disk drives.)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3e476db9",
   "metadata": {
    "id": "3e476db9"
   },
   "source": [
    "The system dynamics can be modeled in python-control using a `NonlinearIOSystem` object, which we create with the `nlsys` function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27bb3c38",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Parameter values\n",
    "servomech_params = {\n",
    "    'J': 100,             # Moment of inertia of the motor\n",
    "    'b': 10,              # Angular damping of the arm\n",
    "    'k': 1,               # Spring constant\n",
    "    'r': 1,               # Location of spring contact on arm\n",
    "    'l': 2,               # Distance to the read head\n",
    "    'eps': 0.01,          # Magnitude of velocity-dependent perturbation\n",
    "}\n",
    "\n",
    "# State derivative\n",
    "def servomech_update(t, x, u, params):\n",
    "    # Extract the configuration and velocity variables from the state vector\n",
    "    theta = x[0]                # Angular position of the disk drive arm\n",
    "    thetadot = x[1]             # Angular velocity of the disk drive arm\n",
    "    tau = u[0]                  # Torque applied at the base of the arm\n",
    "\n",
    "    # Get the parameter values\n",
    "    J, b, k, r = map(params.get, ['J', 'b', 'k', 'r'])\n",
    "\n",
    "    # Compute the angular acceleration\n",
    "    dthetadot = 1/J * (\n",
    "        -b * thetadot - k * r * np.sin(theta) + tau)\n",
    "\n",
    "    # Return the state update law\n",
    "    return np.array([thetadot, dthetadot])\n",
    "\n",
    "# System output (tip radial position + angular velocity)\n",
    "def servomech_output(t, x, u, params):\n",
    "    l = params['l']\n",
    "    return np.array([l * x[0], x[1]])\n",
    "\n",
    "# System dynamics\n",
    "servomech = ct.nlsys(\n",
    "    servomech_update, servomech_output, name='servomech',\n",
    "    params=servomech_params, states=['theta_', 'thdot_'],\n",
    "    outputs=['y', 'thdot'], inputs=['tau'])\n",
    "\n",
    "print(servomech)\n",
    "print(\"\\nParams:\", servomech.params)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "competitive-terrain",
   "metadata": {
    "id": "competitive-terrain"
   },
   "source": [
    "### Linearization\n",
    "\n",
    "To study the open loop dynamics of the system, we compute the linearization of the dynamics about the equilibrium point corresponding to $\\theta_\\text{e} = 15^\\circ$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "senior-carpet",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Convert the equilibrium angle to radians\n",
    "theta_e = (15 / 180) * np.pi\n",
    "\n",
    "# Compute the input required to hold this position\n",
    "u_e = servomech.params['k'] * servomech.params['r'] * np.sin(theta_e)\n",
    "print(\"Equilibrium torque = %g\" % u_e)\n",
    "\n",
    "# Linearize the system about the equilibrium point\n",
    "P = servomech.linearize([theta_e, 0], u_e)[0, 0]\n",
    "# P.update_names(name='linservo')\n",
    "print(\"Linearized dynamics:\\n\", P)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "qGtb17lO4PvM",
   "metadata": {
    "id": "qGtb17lO4PvM"
   },
   "source": [
    "We can check the roots of the characteristic equation for this second order system using the `poles` method (we will learn how this works later in the term):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "Vkji0Y8FT7oq",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check the stability of the equilibrium point\n",
    "P.poles()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "naH-Nl7V4c2R",
   "metadata": {
    "id": "naH-Nl7V4c2R"
   },
   "source": [
    "Alternatively, we can look at the eigenvalues of the \"dynamics matrix\" for the linearized system (we will learn about this formulation in [Lecture 3](cds110-L3_lti-systems.ipynb)):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aKxayyiK4NLj",
   "metadata": {},
   "outputs": [],
   "source": [
    "evals, evecs = np.linalg.eig(P.A)\n",
    "print(evals)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "AYQlD5v9GcK4",
   "metadata": {
    "id": "AYQlD5v9GcK4"
   },
   "source": [
    "Both approaches give the same result and we see that the system is stable (negative real part) with an imaginary component (so we can expect some oscillation in the response)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "instant-lancaster",
   "metadata": {
    "id": "instant-lancaster"
   },
   "source": [
    "### Open loop step response\n",
    "\n",
    "A standard method for understanding the dynamics is to plot output of the system in response to an input that is set to 1 at time $t = 0$ (called the \"step response\").\n",
    "\n",
    "We use the `step_response` function to plot the step response of the linearized, open-loop system and compute the \"rise time\" and \"settling time\" (we will define these more formally next week)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "african-mauritius",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the step response\n",
    "lin_response = ct.step_response(P)\n",
    "timepts, output = lin_response.time, lin_response.outputs\n",
    "\n",
    "# Plot step response (input 0 to output 0)\n",
    "plt.plot(timepts, output)\n",
    "plt.xlabel(\"Time $t$ [ms]\")\n",
    "plt.ylabel(\"Position $y$ [cm]\")\n",
    "plt.title(\"Step response for the linearized, open-loop system\")\n",
    "\n",
    "# Compute and print properties of the step response\n",
    "results = ct.step_info(P)\n",
    "print(\"Rise time:\", results['RiseTime'])              # 10-90% rise time\n",
    "print(\"Settling time:\", results['SettlingTime'])      # 2% error\n",
    "\n",
    "# Calculate the rise time start time by hand\n",
    "rise_time_start = timepts[np.where(output > 0.1 * output[-1])[0][0]]\n",
    "rise_time_stop = rise_time_start + results['RiseTime']\n",
    "\n",
    "# Add lines for the step response features\n",
    "plt.plot([timepts[0], timepts[-1]], [output[-1], output[-1]], 'k--')\n",
    "\n",
    "plt.plot([rise_time_start, rise_time_start], [0, 2.5], 'k:')\n",
    "plt.plot([rise_time_stop, rise_time_stop], [0, 2.5], 'k:')\n",
    "plt.arrow(rise_time_start, 0.5, rise_time_stop - rise_time_start, 0)\n",
    "plt.text((rise_time_start + rise_time_stop)/2, 0.6, '$T_r$')\n",
    "\n",
    "plt.plot([0, 0], [0, 2.5], 'k:')\n",
    "plt.plot([results['SettlingTime'], results['SettlingTime']], [0, 2.5], 'k:')\n",
    "plt.arrow(0, 1.5, results['SettlingTime'], 0)\n",
    "plt.text(results['SettlingTime']/2, 1.6, '$T_s$');\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "DoCK6MWlHaUO",
   "metadata": {
    "id": "DoCK6MWlHaUO"
   },
   "source": [
    "We see that the open loop step response (for the linearized system) is stable, and that the final value is larger than 1 (this value just depends on the parameters in the system)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "nviDlWek9dge",
   "metadata": {
    "id": "nviDlWek9dge"
   },
   "source": [
    "We can also compare the response of the linearized system to the full nonlinear system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "qwrPhD499jbl",
   "metadata": {},
   "outputs": [],
   "source": [
    "nl_response = ct.input_output_response(servomech, timepts, U=1)\n",
    "\n",
    "# Plot step response (input 0 to output 0)\n",
    "plt.plot(timepts, output, label=\"linearized\")\n",
    "plt.plot(timepts, nl_response.outputs[0], label=\"nonlinear\")\n",
    "\n",
    "plt.xlabel(\"Time $t$ [ms]\")\n",
    "plt.ylabel(\"Position $y$ [cm]\")\n",
    "plt.title(\"Step response for the open-loop system\")\n",
    "plt.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7YNmgE2XHmL3",
   "metadata": {
    "id": "7YNmgE2XHmL3"
   },
   "source": [
    "We see that the nonlinear system responds differently.  This is because the force exerted by the spring is nonlinear due to the kinematics of the mechanism design."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "stuffed-premiere",
   "metadata": {
    "id": "stuffed-premiere"
   },
   "source": [
    "## Feedback control design\n",
    "\n",
    "We next design a feedback controller for the system that allows the system to track a desired position $y_\\text{d}$  and sets the closed loop eigenvalues of the linearized system to $\\lambda_{1,2} = −10 \\pm 10 i$.  We will learn how to do this more formally in later lectures, so if you aren't familiar with these techniques, that's OK.\n",
    "\n",
    "We make use of full state feedback of the form $u = -K(x - x_\\text{d})$ where $x_\\text{d}$ is the desired state of the system.  The python-control `place` command can be used to compute the state feedback gains $K$ that set the closed loop poles at a desired location:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8NK8O6XT7B_a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Place the closed loop poles using feedback\n",
    "# u = -K (x - xd)\n",
    "\n",
    "# Find the gains required to place the gains at the desired location\n",
    "K = ct.place(P.A, P.B, [-10 + 10*1j, -10 - 10*1j])\n",
    "print(f\"{K=}\\n\")\n",
    "\n",
    "# Implement an I/O system implementing this control law\n",
    "def statefbk_output(t, x, u, params):\n",
    "  l = params.get('l', 2)\n",
    "  # Create the current and desired state\n",
    "  x = np.array([u[0] / l, u[1]])\n",
    "  xd = np.array([u[2] / l, u[3]])\n",
    "  return -K @ (x - xd)\n",
    "\n",
    "statefbk = ct.nlsys(\n",
    "    None, statefbk_output, name='statefbk',\n",
    "    inputs=['y', 'thdot', 'y_d', 'thdot_d'],\n",
    "    outputs=['tau']\n",
    ")\n",
    "print(statefbk)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "v1fb1pJ_zRLk",
   "metadata": {
    "id": "v1fb1pJ_zRLk"
   },
   "source": [
    "Note that this controller has no internal state, but rather is a static input/output function."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ZR8EKtn-H9V7",
   "metadata": {
    "id": "ZR8EKtn-H9V7"
   },
   "source": [
    "We can now connect the controller to the process using the `interconnect` command.  Because we have named the signals in a careful way, the `interconnect` command can automatically connect everything together:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "associate-assistant",
   "metadata": {},
   "outputs": [],
   "source": [
    "clsys = ct.interconnect(\n",
    "    [servomech, statefbk],\n",
    "    inputs=['y_d', 'thdot_d'],\n",
    "    outputs=['y', 'tau']\n",
    ")\n",
    "print(clsys)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4o5oy_6N51yf",
   "metadata": {
    "id": "4o5oy_6N51yf"
   },
   "source": [
    "To examine the dynamics of the closed loop system, we plot the step response for the closed loop system and compute the rise time, settling time, and steady state error."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "qIEH3Trn53d4",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the step response of the closed loop system\n",
    "timepts = np.linspace(0, 1)\n",
    "clsys_resp = ct.input_output_response(clsys, timepts, [1, 0])\n",
    "\n",
    "plt.plot(clsys_resp.time, clsys_resp.outputs[0])\n",
    "plt.xlabel(\"Time $t$ [ms]\")\n",
    "plt.ylabel(\"Position $y$ [cm]\")\n",
    "plt.title(\"Step response for closed loop, state space controller\")\n",
    "\n",
    "# Compute and print properties of the step response\n",
    "results = ct.step_info(clsys_resp.outputs[0], timepts)\n",
    "print(\"\")\n",
    "print(f\"Rise time: {results['RiseTime']:.2g} ms\")\n",
    "print(f\"Settling time: {results['SettlingTime']:.2g} ms\")\n",
    "print(f\"Steady state error: {abs(results['SteadyStateValue'] - 1) * 100:.2g}%\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "K-ZX_SDmN4rF",
   "metadata": {
    "id": "K-ZX_SDmN4rF"
   },
   "source": [
    "Note the change in timescale (100 ms to 1 ms) and also the fact that the system now goes to the reference value ($y = 1$)."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e0176710",
   "metadata": {
    "id": "e0176710"
   },
   "source": [
    "## Frequency response\n",
    "\n",
    "Another way to measure the performance of the system is to compute its frequency response.\n",
    "\n",
    "Roughly speaking, we set the input of the system to be of the form $u(t) = \\sin(\\omega t)$ and then look at the output signal $y(t)$.  For a *linear* system, we can show that the output signal will have the form\n",
    "\n",
    "$$\n",
    "y(t) = M \\sin(\\omega t + \\phi)\n",
    "$$\n",
    "\n",
    "where the magnitude $M$ and phase $\\phi$ depend on the input frequency.\n",
    "\n",
    "We can plot the magnitude (also called the \"gain\") and the phase of the system as a function of the frequency $\\omega$ and plot these values on a log-log and log-linear scale (called a *Bode* plot):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8684cc1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Compute the linearization of the closed loop system\n",
    "G = clsys.linearize([theta_e, 0], [0, 0], name=\"G\")\n",
    "\n",
    "# Plot the Bode plot (input[0] = yd, outut[0] = y)\n",
    "response = ct.frequency_response(G[0, 0])\n",
    "cplt = response.plot(title=\"Bode plot for G\", freq_label=\"Frequency [rad/ms]\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "W_kzSIKGsSka",
   "metadata": {
    "id": "W_kzSIKGsSka"
   },
   "source": [
    "Examination of the frequency response allows us to identify the range of input frequencies over which the control system can accurately track the input ($M(\\omega) \\approx 1$).  For this system, we have good tracking up to approximately 10 rad/ms, which corresponds to about 1.6 kHz."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "rocky-hobby",
   "metadata": {
    "id": "rocky-hobby"
   },
   "source": [
    "## Trajectory tracking\n",
    "\n",
    "Another type of analysis we might do is to see how well the system can track a more complicated reference trajectory.  For the disk drive example, we might move the system from one point on the disk to a second and then to a third (as we read different portions of the disk).\n",
    "\n",
    "To explore this, we can create simulations of the full nonlinear system with the linear controllers designed above and plot the response of the system.  We do that here for a reference trajectory that has an initial value of 0 cm at $t = 0$, to 1 cm at $t = 0.5$, to 3 cm at $t = 1$, back to 2 cm at $t = 1.5$ ms:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "utility-community",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create a reference trajectory to track\n",
    "timepts = np.linspace(0, 2.5, 250)\n",
    "ref = [\n",
    "    np.concatenate((\n",
    "        np.ones(50) * 0,\n",
    "        np.ones(50) * 1,\n",
    "        np.ones(50) * 3,\n",
    "        np.ones(100) * 2,\n",
    "    )), 0]\n",
    "\n",
    "# Create the system response and plot the results\n",
    "response = ct.input_output_response(clsys, timepts, ref)\n",
    "plt.plot(response.time, response.outputs[0])\n",
    "\n",
    "# Plot the reference trajectory\n",
    "plt.plot(timepts, ref[0], 'k--');\n",
    "\n",
    "# Label the plot\n",
    "plt.xlabel(\"Time $t$ [ms]\")\n",
    "plt.ylabel(\"Position $y$ [cm]\")\n",
    "plt.title(\"Trajectory tracking with full nonlinear dynamics\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "074427a3",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
